#!/usr/bin/env python
# -*- coding: UTF-8 -*-
'''
@Project ：bignet_sumo_dev
@File ：main-dev-tongluoshan.py
@IDE ：PyCharm
@Author ：xinyingjie
@Date ：2025/3/24 9:40
'''

from __future__ import absolute_import
from __future__ import print_function

import asyncio
import base64
import math
import queue
import json
import threading
import random
import time
from multiprocessing import Process, Queue

import traci.constants as tc
import sumolib

from kafka import KafkaProducer, KafkaConsumer
from pyproj import Proj

from config.config_kakou import GLOBAL_POLYGON_CENTER, MAX_SPEED, KAFKA_HOST, \
    RADAR_DATA_LIST, KAFKA_E1_TOPIC, KAFKA_E1_TOPIC_SIMU, SHOW_SIMU, USE_LIBSUMO, USE_GUI, KAFKA_E1_LIST
from utils import sumo_utils
from loguru import logger

from utils.plate_utils import is_valid_license_plate, is_vehicle_passed_gantry, get_radar_paths, \
    is_vehicle_passing_checkpoint
from utils.pursuit_utils import calculate_catch_down_time, haversine, calculate_catch_up_time
from utils.tools import calculate_distance, find_vehicles_by_duration_optimized, \
    calculate_vehicle_acceleration_deceleration, get_timestamp_milliseconds, time_str, \
    get_timestamp_milliseconds_jingtai, get_next_target_car_info_jingtai, is_in_front_jingtai
import concurrent.futures

from websocket.websocketserver1 import WebSocketServer

if USE_LIBSUMO:
    import libsumo as traci
else:
    import traci

logger.add("/sumo/debug.log", format="{time} {level} {message}", level="DEBUG")


def read_trafficE1_channel(target_channel, q_r, kafka_queue, queue_data):
    # 实时接收全域E1帧数据
    kafka_consumer_E1 = KafkaConsumer(
        target_channel,
        bootstrap_servers=KAFKA_HOST,
        auto_offset_reset='latest',
        enable_auto_commit=False
    )
    kafka_producer = KafkaProducer(bootstrap_servers=KAFKA_HOST, key_serializer=str.encode,
                                   value_serializer=lambda x: json.dumps(x).encode('utf-8'),
                                   compression_type='gzip')
    logger.info(f"全域E1 {target_channel} 监听已就绪")
    count_add = 0

    start_time = int(time.time() * 1000)
    kafka_queue.put([target_channel, int(time.time())])
    # 缓存感知11帧数据
    cache_data = Queue(maxsize=2000)
    simu_json_list_before = []
    while True:
        time.sleep(0.1)
        for message in kafka_consumer_E1:
            if message:
                try:
                    sn_set = set()
                    one_list = []
                    message_value = message.value.decode('utf-8')
                    data = json.loads(message_value)
                    q_r.put(data.copy())
                    simu_json_list = []
                    while not queue_data.empty():
                        simu_json_list = queue_data.get()
                    if 'targetList' not in data.keys():
                        continue
                    e1FrameParticipant_list = data['targetList']
                    ts = int(time.time() * 1000)
                    if abs(ts - start_time) > 1000 * 120:
                        start_time = ts
                        kafka_queue.put([target_channel, int(time.time())])
                    for one in e1FrameParticipant_list:
                        sourceId = one['station']
                        one['sourceId'] = sourceId
                        # if sourceId % 2 != 0:
                        sn_set.add(sourceId)
                        # if sourceId != 34:
                        car_id = one['id']
                        dis = (calculate_distance(RADAR_DATA_LIST[sourceId]['lon'],
                                                  RADAR_DATA_LIST[sourceId]['lat'],
                                                  one['lon'], one['lat']))
                        if dis <= 100:
                            one_list.append(one)
                    cache_data.put(one_list)
                    one_list_his = []
                    while cache_data.qsize() > 20:
                        one_list_his = cache_data.get()
                    if len(simu_json_list) > 0:
                        simu_json_list_before = simu_json_list
                    if SHOW_SIMU:
                        data['targetList'] = one_list_his + simu_json_list_before if len(
                            simu_json_list_before) > 0 else one_list_his
                    else:
                        data['targetList'] = one_list_his
                    # kafka_producer.send(KAFKA_E1_TOPIC_SIMU, value=data,
                    #                     key=str(int(time.time())))

                except Exception as e:
                    logger.error(str(e))
                    continue


class SumoMain:
    def __init__(self, sumo_net_file, sumo_cfg_file, gui, process_id):
        """
        仿真初始化
        :param sumo_net_file:  仿真路网文件路径
        :param sumo_cfg_file:  仿真配置文件路径
        :param gui: 是否开启gui
        :param process_id: 进程id
        """

        self.sumo_net_file = sumo_net_file
        self.sumo_cfg_file = sumo_cfg_file
        self.gui = gui
        self.process_id = process_id
        # 仿真路网对象
        self.sumo_net_instance = None
        # 存储卡口、雷达经纬度所在路网的路段和位置信息
        self.kakou_pos_dict = {}
        self.init_sumo()
        self.simu_json_list = []
        self.simu_json_list_before = []
        self.lidar__paths = get_radar_paths(RADAR_DATA_LIST)

        self.sn_car_info_dict = {}
        self.RADAR_sn_set = set(RADAR_DATA_LIST.keys())
        # 存储每一辆车的多帧信息
        self.per_car_info_dict = {}
        # 下游感知车辆已经与上游仿真车辆匹配成功的车辆列表
        self.match_info_dict = {}
        for sn in self.RADAR_sn_set:
            self.sn_car_info_dict[sn] = {}
            self.per_car_info_dict[sn] = {}
            self.match_info_dict[sn] = []
        self.ws_server = WebSocketServer()
        self.ws_server.start()
        self.add_car_station_info = {}
        # 是否开启速度控制
        self.speed_control = False
        # 正在加速列表
        self.speed_accelerate_list = []
        # 正在减速列表
        self.speed_decelerate_list = []
        # 缓存持续时长/原始速度
        self.sim_speed_cache_dict = dict()
        self.car_in_sn_dict = dict()
        self.executor = concurrent.futures.ThreadPoolExecutor(max_workers=300)
        self.global_vehicles = {}
        self.batch_num = 0
        self.global_time = time.time()
        self.global_num = 0
        self.son_process = {}
        self.add_count = 0

    def init_sumo(self):
        """
        初始化sumo  加载
        :return:
        """
        # 加载路网
        self.sumo_net_instance = sumolib.net.readNet(self.sumo_net_file, withInternal=True, withLatestPrograms=True)
        # 启动仿真
        sumo_utils.start_sumo(self.sumo_cfg_file, False, gui=self.gui)
        # 设置所有车道的最大通行速度
        for id in traci.lane.getIDList():
            traci.lane.setMaxSpeed(id, MAX_SPEED)
        # 新增区域中心矩形，用于订阅100KM范围内的所有仿真车辆信息，并配置监听信息内容
        traci.polygon.add('GLOBAL_POLYGON_CENTER', GLOBAL_POLYGON_CENTER, (1, 0, 1))
        traci.polygon.subscribeContext('GLOBAL_POLYGON_CENTER',
                                       tc.CMD_GET_VEHICLE_VARIABLE, 1000000,
                                       [tc.VAR_SPEED, tc.VAR_TYPE, tc.VAR_POSITION, tc.VAR_ANGLE, ])

        # 将雷达、卡口相机的经纬度和终点位置转为路网上所在的路段id 位置距离 车道号，起始点和终点所在的路段id添加路由
        special_gantries = {
            12: {"arrive_edge_id": "-56"},
            16: {"arrive_edge_id": "-65"},
            22: {"arrive_edge_id": "-0"}
        }
        for sn, sn_info in RADAR_DATA_LIST.items():
            # 转换经纬度到路网坐标
            center_x, center_y = traci.simulation.convertGeo(sn_info['lon'], sn_info['lat'], fromGeo=True)
            EdgeID, Pos, LaneIndex = traci.simulation.convertRoad(sn_info['lon'], sn_info['lat'], True)
            next_edge_id, next_pos = sumo_utils.get_downstream_position_simplified(self.sumo_net_instance, EdgeID, Pos,
                                                                                   1)
            if sn_info['sn'] <= 18:
                arrive_edge_id = "-38"
            else:
                arrive_edge_id = "-1"
            routeId2 = None
            next_pos_2 = None
            next_edge_id_2 = None
            if sn in special_gantries:
                # 转换经纬度到路网坐标
                center_x, center_y = traci.simulation.convertGeo(sn_info['lon'], sn_info['lat'], fromGeo=True)
                EdgeID, Pos, LaneIndex = traci.simulation.convertRoad(sn_info['lon'], sn_info['lat'], True)
                # 计算下行位置（与第一套路由保持一致）
                next_edge_id_2, next_pos_2 = sumo_utils.get_downstream_position_simplified(
                    self.sumo_net_instance, EdgeID, Pos, 100)
                # 获取该门架对应的目标路段ID
                special_info = special_gantries[sn]
                arrive_edge_id_2 = special_info["arrive_edge_id"]
                # 创建第二套路由的标识符
                route_id = f"{sn_info['sn']}_2"
                # 添加特殊路由
                traci.route.add(route_id, [str(next_edge_id_2), arrive_edge_id_2])
                routeId2 = route_id
            logger.info(
                f"门架{sn_info['sn']} 所在路段为{EdgeID} 位置为{Pos} 下行100米后的路段为{next_edge_id} 位置为{next_pos}")
            if sn_info['sn'] not in self.kakou_pos_dict.keys():
                traci.route.add(str(sn_info['sn']), [str(next_edge_id), arrive_edge_id])

                self.kakou_pos_dict[str(sn_info['sn'])] = {"EdgeID": next_edge_id, "EdgeID2": next_edge_id_2,
                                                           "Pos": next_pos, "Pos2": next_pos_2,
                                                           'LaneIndex': LaneIndex,
                                                           'routeId': str(str(sn_info['sn'])),
                                                           'routeId2': routeId2}

            # 创建圆形区域，表示相机或雷达的位置，用于在使用gui可视化时，展示设备具体位置
            if sn_info['normal']:
                # 创建圆形区域
                self.create_circle_polygon(sn, center_x, center_y, radius=50)
            else:
                self.create_circle_polygon(sn, center_x, center_y, radius=50, color=(255, 0, 0, 255))

    def create_circle_polygon(self, vehID, center_x, center_y, radius, color=(0, 255, 0, 255), filled=True):
        """
        在指定位置创建一个近似圆形的多边形。

        :param vehID: 车辆 ID 或其他唯一标识符，用于命名多边形
        :param center_x: 圆心的 x 坐标
        :param center_y: 圆心的 y 坐标
        :param radius: 圆的半径
        :param color: 多边形的颜色 (R,G,B,A)，默认绿色不透明
        :param filled: 是否填充多边形，默认 True
        """
        # 生成圆周上的点
        num_points = 50  # 点的数量越多，圆越光滑
        points = []
        for i in range(num_points):
            angle = 2 * math.pi * i / num_points
            x = center_x + radius * math.cos(angle)
            y = center_y + radius * math.sin(angle)
            points.append((x, y))

        # 添加多边形
        polygon_id = f"circle_{vehID}"
        traci.polygon.add(polygon_id, shape=points, color=color, fill=filled, layer=0)

    def control_car(self):
        global_vehicles = self.global_vehicles.copy()
        """车辆发车控制"""
        while not re_queue_data.empty():
            vehicle_info = re_queue_data.get()
            logger.info(f"仿真过程中接收到车辆信息：{vehicle_info}")
            if vehicle_info:
                # 获取车辆信息（对其进行加减速控制）
                if 'picLicense' not in vehicle_info:
                    sn = vehicle_info['station']
                    vehicle_info['picLicense'] = str(sn) + "_" + str(vehicle_info['id'])
                    self.add_camera_car(vehicle_info, sn)
                    continue
                plate = vehicle_info['picLicense']
                if is_valid_license_plate(plate):
                    car_id = base64.b64encode(str(vehicle_info['picLicense']).encode("utf-8")).decode("ascii")
                    sn = vehicle_info['station']
                    # print("原始车辆id", car_id)
                    # print("在网仿真车辆id", set(global_vehicles.keys()))
                    if car_id not in set(global_vehicles.keys()):
                        logger.info("车辆", sn, plate, car_id, "进行发车")
                        self.add_camera_car(vehicle_info, sn)
                    else:
                        veh_info = global_vehicles.get(car_id)
                        pos = veh_info[tc.VAR_POSITION]
                        longitude, latitude = traci.simulation.convertGeo(pos[0], pos[1], fromGeo=False)
                        ang = veh_info[tc.VAR_ANGLE]
                        speed = veh_info[tc.VAR_SPEED]
                        # pass_flag = is_vehicle_passed_gantry(longitude, latitude, ang, vehicle_info['lon'],
                        #                                      vehicle_info['lat'])
                        pass_flag = is_vehicle_passing_checkpoint(longitude, latitude, ang, vehicle_info['lon'],
                                                                  vehicle_info['lat'])
                        # print("2222原始车辆id", car_id)
                        # print("222222在网仿真车辆id", set(global_vehicles.keys()))
                        car_in_sn_info = self.car_in_sn_dict.get(car_id)
                        now_sn = car_in_sn_info.get('now_sn')
                        # if RADAR_DATA_LIST[now_sn] != sn:
                        if pass_flag:
                            result = calculate_catch_down_time(speed, vehicle_info['speed'] / 3.6, 10, 20,
                                                               longitude,
                                                               latitude, vehicle_info['lon'],
                                                               vehicle_info['lat'])
                            t_acc, t_const = result
                            avg_speed = vehicle_info['speed']
                            v_lon = vehicle_info['lon']
                            v_lat = vehicle_info['lat']
                            _distance = haversine(vehicle_info['lon'], vehicle_info['lat'], longitude, latitude)
                            logger.info(
                                f'{plate} 车辆经过门架： {sn}，{longitude}, {latitude}, {v_lon},{v_lat} 距离门架距离为{_distance}米，当前速度为：{speed},'
                                f' 断面速度为  {avg_speed / 3.6} 做减速，减速时间为：{t_acc}  持续时间为：{t_const}')
                            if t_const:
                                # 说明能追上
                                traci.vehicle.slowDown(car_id, speed * 0.8, t_acc)

                                car_in_sn_info["match_sn"] = sn
                                self.car_in_sn_dict[car_id] = car_in_sn_info
                                self.sim_speed_cache_dict[car_id] = {
                                    'sn': sn,
                                    't_acc': t_acc * 1000,
                                    't_const': t_const * 1000,
                                    'time': int(time.time() * 1000),
                                    'i_speed': vehicle_info['speed'] / 3.6,
                                    'flag': 999
                                }
                        else:
                            result = calculate_catch_up_time(speed, vehicle_info['speed'] / 3.6, 5, 20,
                                                             longitude,
                                                             latitude, vehicle_info['lon'],
                                                             vehicle_info['lat'])
                            t_acc, t_const = result
                            avg_speed = vehicle_info['speed']
                            v_lon = vehicle_info['lon']
                            v_lat = vehicle_info['lat']
                            _distance = haversine(vehicle_info['lon'], vehicle_info['lat'], longitude, latitude)
                            logger.info(
                                f'{plate} 车辆未经过门架： {sn}，{longitude}, {latitude}, {v_lon},{v_lat} 距离门架距离为{_distance}米，当前速度为：{speed},'
                                f' 断面速度为  {avg_speed / 3.6} 做加速，加速时间为：{t_acc}  持续时间为：{t_const}')
                            if t_const:
                                # 说明能追上
                                traci.vehicle.slowDown(car_id, speed * 0.8, t_acc)
                                car_in_sn_info = self.car_in_sn_dict.get(car_id)
                                car_in_sn_info["match_sn"] = sn
                                self.car_in_sn_dict[car_id] = car_in_sn_info
                                self.sim_speed_cache_dict[car_id] = {
                                    'sn': sn,
                                    't_acc': t_acc * 1000,
                                    't_const': t_const * 1000,
                                    'time': int(time.time() * 1000),
                                    'i_speed': vehicle_info['speed'] / 3.6,
                                    'flag': 999
                                }
                else:
                    sn = vehicle_info['station']
                    vehicle_info['picLicense'] = str(sn) + "_" + str(vehicle_info['id'])
                    self.add_camera_car(vehicle_info, sn)

    def control_info_car(self):
        """车辆发车控制"""
        # 速度控制
        all_vehicle_keys = self.global_vehicles.copy()
        sim_speed_cache_dict = self.sim_speed_cache_dict.copy()
        for global_car_id, sim_speed_cache in sim_speed_cache_dict.items():
            t_acc = sim_speed_cache['t_acc']
            t_const = sim_speed_cache['t_const']
            s_time = sim_speed_cache['time']
            i_speed = sim_speed_cache['i_speed']
            sn = sim_speed_cache['sn']
            n_time = time.time() * 1000
            if n_time - s_time - t_acc - t_const > 0:
                if global_car_id in all_vehicle_keys:
                    # 速度变化后，移除车辆
                    traci.vehicle.setSpeed(global_car_id, i_speed)
                    logger.info(f'经过门架：{sn}, 车辆{global_car_id} 速度变化结束，恢复初始速度{i_speed}')
                    self.sim_speed_cache_dict.pop(global_car_id)

    def control_delete_car(self):
        """删车控制"""
        # 如果连续3个没有匹配上 则删除车辆
        car_in_sn_dict = self.car_in_sn_dict.copy()
        for car_id, car_info in list(car_in_sn_dict.items()):
            match_sn = car_info['match_sn']
            now_sn = car_info['now_sn']
            # print(car_in_sn_dict)
            for lidar__path in self.lidar__paths:
                if match_sn in lidar__path:
                    match_sn_index = lidar__path.index(match_sn)
                    now_sn_index = lidar__path.index(now_sn)
                    # print(via_sn_list)
                    if abs(now_sn_index - match_sn_index) > 2:
                        logger.info(f'车辆{car_id} 删除，未经过门架：{match_sn}，已经过门架：{now_sn}')
                        traci.vehicle.remove(car_id)
                        self.car_in_sn_dict.pop(car_id)

    def add_camera_car(self, vehiclePicInfo, sn):
        car_id = base64.b64encode(str(vehiclePicInfo['picLicense']).encode("utf-8")).decode("ascii")

        set_speed = vehiclePicInfo['speed'] / 3.6 if 30 <= vehiclePicInfo['speed'] <= 140 else 80 / 3.6
        edge_id, pos, lane_index = traci.simulation.convertRoad(vehiclePicInfo['lon'],
                                                                vehiclePicInfo['lat'],
                                                                True)
        route_ids = traci.route.getIDList()
        if edge_id not in route_ids:
            if int(sn) <= 18:
                arrive_edge_id = "-38"
            else:
                arrive_edge_id = "-1"
            traci.route.add(edge_id, [str(edge_id), arrive_edge_id])

        # if str(sn) not in ["12", "22", "16"]:
        #     routeId = self.kakou_pos_dict[str(sn)]['routeId']
        #     pos = self.kakou_pos_dict[str(sn)]['Pos']
        # else:
        #     if LaneIndex != 0:
        #         routeId = self.kakou_pos_dict[str(sn)]['routeId']
        #         pos = self.kakou_pos_dict[str(sn)]['Pos']
        #     else:
        #         routeId = self.kakou_pos_dict[str(sn)]['routeId2']
        #         pos = self.kakou_pos_dict[str(sn)]['Pos2']
        try:
            traci.vehicle.add(
                vehID=str(car_id),
                routeID=edge_id,
                departPos=pos,
                typeID=1, depart='now',
                departSpeed=str(set_speed),
                departLane='random')
        except Exception as e:
            logger.info(f"加车错误{e}  {lane_index}")
            if 'Invalid departlane' in str(e):
                traci.vehicle.add(
                    vehID=str(car_id),
                    routeID=edge_id,
                    departPos=pos,
                    typeID=1, depart='now',
                    departSpeed=str(set_speed),
                    departLane='random')
        traci.vehicle.setSpeed(str(car_id), set_speed)
        # traci.vehicle.slowDown(str(car_id),int(120 / 3.6),2)
        lon = RADAR_DATA_LIST[sn]['lon']
        lat = RADAR_DATA_LIST[sn]['lat']
        v_lon = vehiclePicInfo['lon']
        v_lat = vehiclePicInfo['lat']
        dis = calculate_distance(lon, lat, v_lon, v_lat)
        print(str(vehiclePicInfo['picLicense']), edge_id, pos, set_speed, "发车距离门架：", dis, "米")
        vehiclePicInfo['id'] = str(car_id)
        vehiclePicInfo['sourceId'] = sn
        vehiclePicInfo['picLicense'] = car_id
        traci.vehicle.setParameter(str(car_id), "etc_info",
                                   str(vehiclePicInfo))
        # 计算发车位置距离门架多远
        via_list = list()
        self.car_in_sn_dict[car_id] = {
            "start_sn": sn,
            "now_sn": sn,
            "match_sn": sn,
            "via_sn_list": via_list.append(sn)
        }
        # except Exception as e:
        #     logger.error(f"加车异常{str(e)}")

    def stop_remove_car(self, global_vehicles, queue_data):
        # print(queue_data.qsize())
        simu_json_list = []
        objs = []
        sim_speed_cache_dict = self.sim_speed_cache_dict.copy()
        for vehicle_id, veh_info in dict(global_vehicles).items():
            pos = veh_info[tc.VAR_POSITION]
            longitude, latitude = traci.simulation.convertGeo(pos[0], pos[1], fromGeo=False)
            course_angle = veh_info[tc.VAR_ANGLE]
            speed = veh_info[tc.VAR_SPEED]
            # 计算车辆当前位置
            car_in_sn_info = self.car_in_sn_dict.get(vehicle_id)
            # print(f"车辆{vehicle_id} ：{car_in_sn_info}")
            if not car_in_sn_info:
                print(vehicle_id)
                continue
            # 获取车辆当前所在车道 以及车道信息
            self.assign_lane_by_speed(vehicle_id, speed * 3.6)

            match_sn = car_in_sn_info["match_sn"]
            via_sn_list = car_in_sn_info['via_sn_list']
            sn_info = RADAR_DATA_LIST.get(match_sn)

            if sn_info["Downstream"]:
                down_sn = sn_info["Downstream"]['sn']
                down_lon = RADAR_DATA_LIST[down_sn]['lon']
                down_lat = RADAR_DATA_LIST[down_sn]['lat']
                pass_flag = is_vehicle_passing_checkpoint(longitude, latitude, course_angle, down_lon,
                                                          down_lat)
                # print(f"车辆{vehicle_id}是否过门架：{down_sn} {pass_flag}")
                if pass_flag:
                    car_in_sn_info['now_sn'] = down_sn
                    self.car_in_sn_dict[vehicle_id] = car_in_sn_info
                    # print(f"车辆{vehicle_id}已过门架：{down_sn} 当前途径门架列表为：{car_in_sn_info['via_sn_list']}")

            # traci.vehicle.remove(vehicle_id) if int(speed * 3.6) < 20 else 1
            # vehicle_type = veh_info[tc.VAR_TYPE]
            # 判断当前行为
            flag = 1
            if vehicle_id in sim_speed_cache_dict.keys():
                sim_speed_info = sim_speed_cache_dict.get(vehicle_id)
                flag = sim_speed_info['flag']
            etc_info = eval(traci.vehicle.getParameter(vehicle_id, "etc_info"))
            laneNum = 0
            decoded = base64.b64decode(vehicle_id).decode("utf-8")
            temp = {"orgCode": "simu", "id": vehicle_id, "carType": etc_info['carType'],
                    "lon": longitude, "lat": latitude, "speed": speed * 3.6, "angle": round(course_angle, 1),
                    "station": etc_info['station'], "lane": etc_info['lane'],
                    "enGap": 0, "speedAvg": 0, "passTime": 0,
                    "firstReceiveTime": time_str(int(time.time() * 1000)), "disBefore": 0
                    }
            obj = {'id': decoded, 'type': etc_info['carType'], 'source': etc_info['station'],
                   'laneNum': etc_info['lane'], 'longitude': longitude, 'latitude': latitude,
                   'speed': speed * 3.6, 'courseAngle': course_angle,
                   'plate': decoded, 'flag': flag, 'orgCode': 'JJ-10-01'}
            simu_json_list.append(temp)
            objs.append(obj)
        ws_data = {"frameNum": self.batch_num, "globalTime": time_str(int(time.time() * 1000)), "obj": objs}
        frame_info = {"orgCode": "simu", "frameNum": self.batch_num, "globalTime": time_str(int(time.time() * 1000)),
                      "targetList": simu_json_list, }
        # frame_info = {"orgCode": "simu", "frameNum": self.batch_num, "globalTime": time_str(int(time.time() * 1000)),
        #               "targetList": simu_json_list, }
        # print("仿真端", len(simu_json_list))
        if len(simu_json_list) > 0:
            queue_data.put(frame_info)
            future = asyncio.run_coroutine_threadsafe(self.ws_server.broadcast(str(ws_data).replace("'", '"')),
                                                      self.ws_server.loop)
            future.result()  # 等待协程完成

    # def assign_lane_by_speed(self, veh_id, current_speed):
    #     road_id = traci.vehicle.getRoadID(veh_id)
    #     num_lanes = traci.edge.getLaneNumber(road_id)
    #     current_lane = traci.vehicle.getLaneIndex(veh_id)
    #
    #     # 检查前方100米内是否有车辆（无车则保持车道）
    #     if traci.vehicle.getLeader(veh_id, 100) is None:
    #         return
    #
    #     # 定义目标车道（注意：索引越大越靠右）
    #     if num_lanes == 3:
    #         if current_speed >= 100:
    #             target_lanes = [num_lanes - 1, num_lanes - 2]  # 高速靠左 → 右数第0,1车道
    #         else:
    #             target_lanes = [num_lanes - 2, num_lanes - 3]  # 低速靠右 → 右数第1,2车道
    #
    #     elif num_lanes == 4:
    #         if current_speed > 120:
    #             target_lanes = [num_lanes - 1, num_lanes - 2]  # 最左侧两车道（右数0,1）
    #         elif current_speed >= 100:
    #             target_lanes = [num_lanes - 2, num_lanes - 3]  # 中间偏左（右数1,2）
    #         elif current_speed >= 80:
    #             target_lanes = [num_lanes - 2, num_lanes - 3]  # 中间（右数1,2）
    #         else:
    #             target_lanes = [num_lanes - 3, num_lanes - 4]  # 最右侧两车道（右数2,3）
    #
    #     elif num_lanes >= 5:
    #         if current_speed > 120:
    #             target_lanes = [num_lanes - 1, num_lanes - 2]  # 最左两车道（右数0,1）
    #         elif current_speed >= 100:
    #             target_lanes = [num_lanes - 2, num_lanes - 3]  # 左中（右数1,2）
    #         elif current_speed >= 80:
    #             target_lanes = [num_lanes - 3, num_lanes - 4]  # 中右（右数2,3）
    #         else:
    #             target_lanes = [num_lanes - 4, num_lanes - 5]  # 最右两车道（右数3,4）
    #     else:
    #         return  # 车道数不足3时不处理
    #
    #     # 转换车道索引为SUMO实际索引（从左到右0~n-1）
    #     target_lanes = [num_lanes - 1 - lane for lane in target_lanes]
    #
    #     # 如果当前车道不在目标范围内，则换到最近的目标车道
    #     if current_lane not in target_lanes:
    #         nearest_lane = min(target_lanes, key=lambda x: abs(x - current_lane))
    #         traci.vehicle.changeLaneRelative(veh_id, nearest_lane, 3)  # 3秒内完成换道
    def assign_lane_by_speed(self, veh_id, current_speed):
        road_id = traci.vehicle.getRoadID(veh_id)
        num_lanes = traci.edge.getLaneNumber(road_id)
        current_lane = traci.vehicle.getLaneIndex(veh_id)

        # 检查前方100米内是否有车辆（无车则保持车道）
        leader = traci.vehicle.getLeader(veh_id, 200)
        if leader is None:
            # 无前车时，随机决定是否换道（模拟自然分散）
            if random.random() < 0.2:  # 20%概率随机换道
                target_lane = random.randint(0, num_lanes - 1)
                if target_lane != current_lane:
                    traci.vehicle.changeLane(veh_id, target_lane, 3)
            return

        # 有前车时才按速度规则换道
        if num_lanes == 3:
            if current_speed >= 100:
                # 高速车辆可以分布在左或中车道（右数0或1）
                target_lanes = [num_lanes - 1, num_lanes - 2] if random.random() < 0.7 else [num_lanes - 2]
            else:
                # 低速车辆在右侧车道（右数2）
                target_lanes = [num_lanes - 3]

        elif num_lanes == 4:
            if current_speed > 120:
                # 超高速车辆在最左车道（右数0），但有30%概率选择左二车道
                target_lanes = [num_lanes - 1] if random.random() < 0.7 else [num_lanes - 2]
            elif current_speed >= 100:
                # 高速车辆主要在左二车道（右数1），可能延伸到左三
                target_lanes = [num_lanes - 2] if random.random() < 0.8 else [num_lanes - 3]
            elif current_speed >= 80:
                # 中速车辆主要在右二车道（右数2）
                target_lanes = [num_lanes - 3]
            else:
                # 低速车辆在最右车道（右数3）
                target_lanes = [num_lanes - 4]

        elif num_lanes >= 5:
            if current_speed > 120:
                # 超高速车辆在最左两车道随机分布
                target_lanes = random.choice([[num_lanes - 1], [num_lanes - 2]])
            elif current_speed >= 100:
                # 高速车辆在左三、左四车道随机分布
                target_lanes = random.choice([[num_lanes - 2], [num_lanes - 3]])
            elif current_speed >= 80:
                # 中速车辆在中右车道
                target_lanes = [num_lanes - 3, num_lanes - 4]
            else:
                # 低速车辆在最右两车道
                target_lanes = [num_lanes - 4, num_lanes - 5]
        else:
            return  # 车道数不足3时不处理

        # 转换车道索引为SUMO实际索引（从左到右0~n-1）
        target_lanes = [num_lanes - 1 - lane for lane in target_lanes]

        # 如果当前车道不在目标范围内，则换到最近的目标车道
        if current_lane not in target_lanes:
            # 从目标车道中随机选择一个（增加分散性）
            target_lane = random.choice(target_lanes)
            traci.vehicle.changeLaneRelative(veh_id, target_lane, 3)  # 3秒内完成换道

    def main_loop(self, stop_data, time_data):
        global re_queue_data, kafka_queue

        kafka_queue = Queue(maxsize=2000)
        # 在子进程中初始化多进程队列和资源
        queue_data = Queue(maxsize=2000)  # 多进程队列在此处创建
        # Process(target=send_kafka_1, args=(queue_data, self.process_id,)).start()
        # 在子进程中初始化多进程队列和资源
        re_queue_data = Queue(maxsize=2000)  # 多进程队列在此处创建
        kafka_time_dict = {}

        kafka_time_dict[KAFKA_E1_LIST] = int(time.time())
        son_process = Process(target=read_trafficE1_channel,
                              args=(KAFKA_E1_LIST, re_queue_data, kafka_queue, queue_data), daemon=True)
        son_process.start()
        self.son_process[KAFKA_E1_LIST] = son_process
        # son_process = Process(target=senf_kafka_E1,
        #                       args=(queue_data, self.process_id), daemon=True)
        # son_process.start()
        # self.son_process['send'] = son_process
        low_frame_num = 0
        batch_num = 0
        all_set = set()

        while True:
            # try:
            start_time = time.time()
            start_time_m = int(time.time() * 1000)

            # ================================收到门架上报数据，进行车辆处理==========================================

            global_vehicles = traci.polygon.getContextSubscriptionResults('GLOBAL_POLYGON_CENTER')
            self.global_vehicles = global_vehicles
            self.control_car()
            self.control_info_car()
            self.control_delete_car()
            self.stop_remove_car(global_vehicles, queue_data)
            self.batch_num = batch_num

            batch_num += 1
            traci.simulationStep()
            end_time = time.time()
            all_set.update(set(global_vehicles.keys()))
            logger.info(f"仿真数量总数{len(all_set)}")
            if end_time - self.global_time > 1:
                while kafka_queue.qsize() > 0:
                    kafka_info = kafka_queue.get()
                    kafka_topic = kafka_info[0]
                    ts = kafka_info[1]
                    kafka_time_dict[kafka_topic] = ts

                # if abs(kafka_time_dict[KAFKA_E1_LIST] - end_time) > 300:
                #     if KAFKA_E1_LIST in self.son_process.keys():
                #         logger.info(f"topic为{KAFKA_E1_LIST} 的接收进程,超过两分钟未收到数据，重启服务")
                #         s_p = self.son_process[KAFKA_E1_LIST]
                #         s_p.terminate()
                #         s_p.join()
                #         son_process = Process(target=read_trafficE1_channel,
                #                               args=(KAFKA_E1_LIST, re_queue_data, kafka_queue, queue_data), daemon=True)
                #         son_process.start()
                #         self.son_process[KAFKA_E1_LIST] = son_process

                time_data.put(int(time.time()))
                simu_frame = abs(self.global_num - batch_num)
                logger.info(f"进程{self.process_id} 仿真帧率 {simu_frame} 仿真数量{len(global_vehicles)}")
                if simu_frame < 7:
                    low_frame_num += 1
                # if low_frame_num > 10 or (self.global_num > 8000 and len(global_vehicles) == 0):
                #     logger.info(f"关闭子进程")
                #     for s_name, s_p in self.son_process.items():
                #         s_p.terminate()
                #         s_p.join()
                #     stop_data.put(1)
                self.global_time = end_time
                self.global_num = batch_num
            if start_time and (end_time - start_time) < 0.1:
                # print("耗时",(end_time - start_time))
                time.sleep(0.1 - (end_time - start_time))
            else:
                if start_time:
                    # logger.info(f"单帧耗时大于0.1s {end_time - start_time}")
                    pass
            # except Exception as e:
            #     logger.error(f"主循环异常{str(e)}")
            #     continue


def run_sumo_simulation(sumo_net_file, sumo_cfg_file, gui, process_id, stop_data, time_data):
    sumo_main = SumoMain(sumo_net_file, sumo_cfg_file, gui, process_id)
    sumo_main.main_loop(stop_data, time_data)


if __name__ == '__main__':
    # # 路网文件
    # sumo_net_file = "/sumo/net/jingtai.net.xml0611"
    # # 配置文件
    # sumo_cfg_file = "/sumo/net/jingtai.sumocfg"
    # 路网文件
    sumo_net_file = "./net/jingtai.net.xml"
    # 配置文件
    sumo_cfg_file = "./net/jingtai.sumocfg"
    # 是否启用gui可视化界面，调试阶段启用，线上部署关闭减少资源占用
    stop_data = Queue(maxsize=2000)  # 创建队列，仿真循环里面判断，如果仿真帧率小于7或者仿真路网上仿真车流数量为零，向队列中添加数据
    time_data = Queue(maxsize=2000)  # 创建队列，仿真主循环里一秒向里面添加一次系统时间戳
    gui = USE_GUI
    for i in range(1):
        logger.info("开启线程")
        tess_process = Process(target=run_sumo_simulation,
                               args=(sumo_net_file, sumo_cfg_file, gui, i, stop_data, time_data))
        tess_process.start()
        time_simu = int(time.time())
        # while True:
        #     stop_flag = False
        #     # 如果仿真的最新时间与当前系统的时间的时间差大于一分钟，认为仿真异常关闭，需要重启服务
        #     while time_data.qsize() > 0:
        #         time_simu = time_data.get()
        #     if time_simu:
        #         # logger.info(f"时间差{abs(time_simu - int(time.time()))}")
        #         if abs(time_simu - int(time.time())) > 60:
        #             stop_flag = True
        #             time_simu = int(time.time())
        #     # 如果仿真帧率小于7或者仿真路网上仿真车流数量为零，认为仿真异常，需要重启服务
        #     while stop_data.qsize() > 0:
        #         _ = stop_data.get()
        #         stop_flag = True
        # if stop_flag:
        #     logger.info("关闭开启线程")
        #     tess_process.terminate()
        #     tess_process.join()
        #     logger.info("重新开启线程")
        #     tess_process = Process(target=run_sumo_simulation,
        #                            args=(sumo_net_file, sumo_cfg_file, gui, i, stop_data, time_data))
        #     tess_process.start()
        # time.sleep(1)
